#include <joyos.h>

#include "goalscore.h"
#include "define.h"
#include "led.h"

/**
 * readScore interpretes the raw value read from the frob knob into the actual score.
 * The thresholds between scores are predefined.
 * 
 * @requires none
 * @effect an actual score that corresponds to the raw value is returned
 * @modifies none
 */
uint16_t readScore(uint16_t frob_val){
	if (frob_val < THRESHOLD_15_TO_17)
		return 15;
	else if ((frob_val >= THRESHOLD_15_TO_17) && (frob_val < THRESHOLD_17_TO_18))
		return 17;
	else if ((frob_val >= THRESHOLD_17_TO_18) && (frob_val < THRESHOLD_18_TO_19))
		return 18;
	else if ((frob_val >= THRESHOLD_18_TO_19) && (frob_val < THRESHOLD_19_TO_21))
		return 19;
	else if ((frob_val >= THRESHOLD_19_TO_21) && (frob_val < THRESHOLD_21_TO_22))
		return 21;
	else
		return 22;
	
	//return 0;
}

/**
 * getGoalScore allows user to adjust for the goal score of the game with frob knob.
 * While adjusting the frob knob, the currently chosen value will be printed in screen.
 * After a value is chosen, press "GO" to continue.
 *
 * @requires none
 * @effect set # of balls to go in each goal 
 * @modifies pointToDo
 */
void getGoalScore(){
	uint16_t frobValue;
	uint8_t score = 0;
	
	printf_P(PSTR("\nChoose score"));
	go_click();
	
	while (!stop_press()){
		frobValue = frob_read();
		score = readScore(frobValue);
		printf_P(PSTR("\ngoal point = %d (click STOP)"), score);
		pause(200);
	}
	
	goalPoint = score;
	
	switch (goalPoint) {
		case 15:
			pointToDo[0] = 2;
			pointToDo[1] = 3;
			pointToDo[2] = 1;
			break;
		case 17:
			pointToDo[0] = 3;
			pointToDo[1] = 2;
			pointToDo[2] = 1;
			break;
			
		case 18:
			pointToDo[0] = 3;
			pointToDo[1] = 3;
			pointToDo[2] = 0;
			break;
			
		case 19:
			pointToDo[0] = 4;
			pointToDo[1] = 1;
			pointToDo[2] = 1;
			break;

		case 21:
			pointToDo[0] = 5;
			pointToDo[1] = 1;
			pointToDo[2] = 0;
			break;
		
		case 22:
			pointToDo[0] = 5;
			pointToDo[1] = 1;
			pointToDo[2] = 0;
			break;
			
		default:
			pointToDo[0] = 3;
			pointToDo[1] = 3;
			pointToDo[2] = 0;
			break;
	}
		
}

/**
 * "calibrateGoalServo" calibrates the goal servo to the starting state (the state where the
 * ball handler part is directed to the right.
 *
 * @requires none
 * @effects the servo for scoring wheel is calibrated to the right position
 * @modifies goal servo position adjusts to position 511
 */
void calibrateGoalServo(){
	printf_P (PSTR("\nWheel Goal to the Right"));
	go_click();
	servo_set_pos(SERVO_SCORE, 511);
	printf_P (PSTR("\nPut it back"));
	go_click();
}

/**
 * "scoreLeft" shoots the ball through the left scoring tray with the given amount
 *
 * @requires none
 * @effects the given number of balls are shooted out from the left goal
 * @modifies goal servo position adjusts to position 0
 */
void scoreLeft(uint8_t numBalls){
	for(int i=0;i<numBalls;i++){
		servo_set_pos(SERVO_SCORE, 210);
		pause(1000);
		servo_set_pos(SERVO_SCORE, 0);
		pause(1000);		
	}
}

/**
 * "scoreRight" shoots the ball through the right scoring tray with the given amount
 *
 * @requires none
 * @effects the given number of balls are shooted out from the right goal
 * @modifies goal servo position adjusts to position 0
 */
void scoreRight(uint8_t numBalls){
	for(int i=0;i<numBalls;i++){
		servo_set_pos(SERVO_SCORE, 240);
		pause(1000);
		servo_set_pos(SERVO_SCORE, 511);
		pause(1000);
	}
}

void score (uint8_t whichside, uint8_t numBalls) {
	if (whichside == 1) {
		scoreLeft(numBalls);
	}
	
	else if (whichside == 0) {
		scoreRight(numBalls);
	}
	
	else {
		printf ("score ERROR");
	}	

}
	
uint8_t isGoalOnTheLeft(void) {
	int count = 0;
	for(int i = 0; i<6; i++){
		if(isBlack(LED_LEFT_FRONT) && isBlack(LED_LEFT_BACK)){
			count++;
		}
	}
	return count > 3 ? 1 : 0;
}

uint8_t isGoalOnTheRight(void) {
	int count = 0;
	for(int i = 0; i<6; i++){
		if(isBlack(LED_RIGHT_FRONT) && isBlack(LED_RIGHT_BACK)){
			count++;
		}
	}
	return count > 3 ? 1 : 0;
}

uint8_t isGoal(uint8_t whichside) {		// 1: left, 0: right
	if (whichside == 1) {
		return isGoalOnTheLeft();
	}
	
	else if (whichside == 0) {
		return isGoalOnTheRight();
	}
	
	else {
		printf ("isGoal ERROR");
		return 0;
	}
}
